#include <p32xxxx.h>
#include "driverESC_I2C_ISR.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
#include "semphr.h"
#include "main_peripherals.h"
#include "rtos_queues.h"
#include "state.h"
#include "config.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "tools.h"
#include "driverESC_I2C_Support.h"
#include "commands.h"

extern SemaphoreHandle_t semESCPulse;

BYTE escCmdBuffer[256];
int escCmdParams = 0;



typedef enum
{
    STATE_START,
    STATE_ADDRESS,
    STATE_SETPOINT_MSB,
    STATE_SETPOINT_LSB,
    STATE_REPEAT_START,
    STATE_ADDRESS_READ,
    STATE_READ,
    STATE_READ_ACK,
    STATE_STOP,
    STATE_STOPPED,
    STATE_ERROR_STOP,
    STATE_ERROR
} ESC_I2C_STATE;

struct
{
    ESC_I2C_STATE state;
    
    unsigned char motorAddress;
    short motorSetpoint;

    unsigned char numFeedbackVars;
    unsigned char numFeedbackCurrent;

    unsigned char motorFeedback[7];

    unsigned char type;
} escI2CISR;

#include <math.h>

void ESC_INFO()
{
    if (escCmdParams != 1) return;
    BYTE motorId = escCmdBuffer[0];

    if (escI2CWriteBytes(motorId, 0xF0, NULL, 0, TRUE) == 0) return;
    if (rnClaimTx(configTICK_RATE_HZ))
    {
        BYTE * packet = rnGetTxBuffer();
        packet[0] = CMD_ESC_INFO;
        packet[1] = motorId;
        packet[2] = STATE.motors.address[motorId];
        escI2CReadByte(TRUE, packet+3);   // bootloader
        escI2CReadByte(TRUE, packet+4); // firmware b1
        escI2CReadByte(TRUE, packet+5); // firmware b2
        escI2CReadByte(TRUE, packet+6); // pic b1
        escI2CReadByte(TRUE, packet+7); // pic b2
        escI2CReadByte(TRUE, packet+8); // pic b3
        escI2CReadByte(TRUE, packet+9); // pic b4
        escI2CStop();
        rnTransmitAndConfirmBlock(packet, 10);
        rnReleaseTx();
    }
}


void ESC_SETTINGS_WRITE()
{
    if (escCmdParams != 1) return;

    escI2CWriteBytes(escCmdBuffer[0], 0x03, NULL,0, FALSE);

    if (!rnClaimTx(configTICK_RATE_HZ)) return;
    BYTE * packet = rnGetTxBuffer();
    packet[0] = CMD_ESC_SETTINGS_WRITE;
    packet[1] = escCmdBuffer[0];
    rnTransmitAndConfirmBlock(packet, 2);
    rnReleaseTx();
}

void ESC_SETTING_SET()
{
    if (escCmdParams != 4) return;
    BYTE motorId = escCmdBuffer[0];

    if (escIsBootloader(motorId) == TRUE) return;

    escI2CWriteBytes(motorId, 0x02, escCmdBuffer+1, 3, FALSE);

    if (!rnClaimTx(configTICK_RATE_HZ)) return;
    BYTE * packet = rnGetTxBuffer();
    packet[0] = CMD_ESC_SETTING_SET;
    packet[1] = escCmdBuffer[0];
    rnTransmitAndConfirmBlock(packet, 2);
    rnReleaseTx();
}

void ESC_SETTINGS()
{
    if (escCmdParams != 1) return;

    BYTE motorId = escCmdBuffer[0];
    
    if (!rnClaimTx(configTICK_RATE_HZ)) return;
    BYTE * packet = rnGetTxBuffer();
    unsigned int pos = 3;
    packet[0] = CMD_ESC_SETTINGS;
    packet[1] = escCmdBuffer[0];
    packet[2] = STATE.motors.address[escCmdBuffer[0]];

    if (escIsBootloader(motorId) == FALSE)
    {
        if (escI2CWriteBytes(motorId, 0x01, NULL, 0, TRUE) == 1)
        {
            int ff_count = 0;
            while (ff_count < 4)
            {
                escI2CReadByte(TRUE, packet + pos);
                if (packet[pos] == 0xFF) ff_count++; else ff_count = 0;
                pos++;
            }
            escI2CReadByte(FALSE, packet + pos);
            escI2CStop();

            rnTransmitAndConfirmBlock(packet, pos);
        }
    }

    rnReleaseTx();
}


void escScanBus()
{
    STATE.motors.count = 0;

    int i;
    for (i=0;i<8;i++)
    {
        STATE.motors.address[i] = 0;
        STATE.motors.in_bootloader[i] = 0;
    }

    BYTE count = 0;
    BYTE b1;

    for (i = 0x90; i <= 0x9E; i+= 2)
    {
        while (escI2CStart() != SUCCESS)
        {
            i2cChannelReset(0);
            vTaskDelay(10);
        }
        if (escI2CSendByte(i) == ACK)
        {
            if (escI2CSendByte(0xFE) == ACK)
            {// bootloader bytes!
                escI2CRestart();
                if (escI2CSendByte(i|1) == ACK) // read address
                {
                    escI2CReadByte(FALSE, &b1);

                    BYTE mapping = CONFIG.base.motorMapping[count++];
                    if (mapping == 0) continue;

                    STATE.motors.address[mapping-1] = i;
                    STATE.motors.in_bootloader[mapping-1] = (b1 == 1) ? 1 : 0;
                    STATE.motors.count++;
                }
            }
        }
        escI2CStop();
        vTaskDelay(1);
    }
}

void ESC_FIRMWARE_FLASH_ROW()
{
    if (escCmdParams != 4 + 192) return;
    BYTE motorId = escCmdBuffer[0];
    if (STATE.motors.mode != MOTOR_MODE_OFF) escSetModeOff();
    if (escIsBootloader(motorId) == FALSE) return;

    int orgBRG = I2C1BRG;
    I2C1BRG = configPERIPHERAL_CLOCK_HZ / (2 * 100000) - 2; // TODO: +/-, not correcting for PGD (104ns)
    //I2CSetFrequency(I2C1,configPERIPHERAL_CLOCK_HZ,100000);
    BYTE result = 0;
    if (escI2CWriteBytes(motorId, 0x62, escCmdBuffer+1, 3+192, TRUE) == 1)
    {
        BYTE response;
        escI2CReadByte(FALSE, &response);
        escI2CStop();
        if (response == 0xA5) result = 1;
    }
    I2C1BRG = orgBRG;

    if (!rnClaimTx(configTICK_RATE_HZ)) return;
    BYTE * sendpacket = rnGetTxBuffer();
    sendpacket[0] = CMD_ESC_FIRMWARE_FLASH_ROW;
    sendpacket[1] = motorId;
    sendpacket[2] = result;
    rnTransmitAndConfirmBlock(sendpacket, 3);
    rnReleaseTx();
}

BOOL escIsArmed(BYTE motorId)
{
    if (escI2CWriteBytes(motorId, 0x30, NULL, 0, TRUE) == 0) return FALSE;
    BYTE b1;
    escI2CReadByte(FALSE, &b1);
    escI2CStop();
    return b1;
}

BOOL escArm(BYTE motorId, BYTE arm)
{
    if (escIsArmed(motorId) == arm) return TRUE;
    if (escI2CWriteBytes(motorId, 0x30, &arm, 1, FALSE) == 0) return FALSE;
    if (escIsArmed(motorId) == arm) return TRUE;
    return FALSE;
}

BOOL escIsBootloader(BYTE motorId)
{
    if (escI2CWriteBytes(motorId, 0xFE, NULL, 0, TRUE) == 0) return FALSE;
    BYTE b1;
    escI2CReadByte(FALSE, &b1);
    STATE.motors.in_bootloader[motorId] = (b1 == 1) ? 1 : 0;
    escI2CStop();
    return (STATE.motors.in_bootloader[motorId]); // == enter) return TRUE;
//    return FALSE;
}

BOOL escBootloader(BYTE motorId, BYTE enter)
{
    if (escIsBootloader(motorId) == enter) return TRUE;

    if (escI2CWriteBytes(motorId, enter == 0 ? 0xFD : 0xFB, NULL, 0, FALSE) == 0) return FALSE;
    vTaskDelay(configTICK_RATE_HZ / 10);

    if (escIsBootloader(motorId) == enter) return TRUE;
}

int escControlFreq()
{
    return 1000;
}


void escSetModeOffDelay(int delay)
{
    int i;
    for (i=0;i<8;i++)
    {
        if (STATE.motors.address[i] == 0) continue;
        STATE.motors.setpoints[i].setPointRPM = 0;
    }

    escSetSpeed();
    escSetSpeed();
    escSetSpeed();
    escSetSpeed();

    for (i=0;i<8;i++)
    {
        if (delay > 0) vTaskDelay(delay);
        escBootloader(i, 0);
        escArm(i, 0);
    }

    STATE.motors.mode = MOTOR_MODE_OFF;
}

void escSetModeOff()
{
    escSetModeOffDelay(0);
}

void ESC_START_BODE()
{
    if (escCmdParams != 7) return;
    unsigned char motor = escCmdBuffer[0];
    short frequency = (escCmdBuffer[1] << 8) | escCmdBuffer[2];
    short amplitude = (escCmdBuffer[3] << 8) | escCmdBuffer[4];
    short duration = (escCmdBuffer[5] << 8) | escCmdBuffer[6];
    
    if (STATE.motors.mode != MOTOR_MODE_DEBUG) return;
    if (STATE.motors.debugSelected != motor) return;
    if (STATE.motors.in_bootloader[motor]) return;
    if (STATE.motors.tuning.setpoint == 0) return;

    if (bodeStart(frequency, amplitude, duration, 0))
    {
        int controlFreq = escControlFreq(motor);

        STATE.bode.controlFrequency = controlFreq;

        STATE.motors.mode = MOTOR_MODE_BODE;
    }
}

void escSetModeFlight()
{
    int i;
    for (i=0;i<8;i++)
    {
        if (STATE.motors.address[i] == 0) continue;

        if (escBootloader(i, 0) == FALSE)
        {
            escSetModeOff();
            return;
        }
        if (escArm(i, 1) == FALSE)
        {
            escSetModeOff();
            return;
        }
    }

    STATE.motors.mode = MOTOR_MODE_FLIGHT;
}

void escSetModeDebug(BYTE motor)
{
    if (STATE.motors.mode == MOTOR_MODE_BODE) return;
    if (STATE.motors.mode == MOTOR_MODE_DEBUG &&
        STATE.motors.debugSelected == motor) return;

    int i;
    for (i=0;i<8;i++)
    {
        if (i == motor) continue;
        if (STATE.motors.address[i] == 0) continue;
        escArm(i,0);
    }
    if (escBootloader(motor, 0) == FALSE ||
        escArm(motor, 1) == FALSE)
    {
        escSetModeOff();
        return;
    }

    STATE.motors.debugSelected = motor;
    STATE.motors.mode = MOTOR_MODE_DEBUG;
}
void ESC_SET_DEBUG_RPM()
{
    if (escCmdParams != 3)
    {
        escSetModeOff();
        return;
    }
    BYTE motor = escCmdBuffer[0];
    STATE.motors.setpoints[motor].setPointRPM = (escCmdBuffer[1] << 8) | escCmdBuffer[2];
    escSetModeDebug(motor);
}

void escSetSpeed()
{
    if (STATE.motors.mode == MOTOR_MODE_OFF) return;
    
    escBusEnableISR();

    int i;
    for (i=0;i<8;i++)
    {
        if (STATE.motors.address[i] == 0) continue;
        if (STATE.motors.in_bootloader[i]) continue;

        if (STATE.motors.mode == MOTOR_MODE_FLIGHT)
        {
            escI2CISR.numFeedbackVars = 2;
            escI2CISR.motorAddress = STATE.motors.address[i] & 0x3F;
            escI2CISR.motorSetpoint = STATE.motors.setpoints[i].setPointRPM;
        }
        else if (STATE.motors.mode == MOTOR_MODE_DEBUG)
        {
            if (i != STATE.motors.debugSelected) continue;
            escI2CISR.numFeedbackVars = 6;
            escI2CISR.motorAddress = STATE.motors.address[i] & 0x3F;
            escI2CISR.motorSetpoint = STATE.motors.setpoints[i].setPointRPM;
        }
        else if (STATE.motors.mode == MOTOR_MODE_BODE)
        {
            if (i != STATE.motors.debugSelected) continue;
            escI2CISR.numFeedbackVars = 7;
            escI2CISR.motorAddress = STATE.motors.address[i] & 0x3F | 0x40;
            escI2CISR.motorSetpoint = STATE.bode.value;
        }

        if (CONFIG.base.enableMotorOutput == 0)
            escI2CISR.motorSetpoint = 0;

        escI2CISR.state = STATE_START;
        escI2CISR.motorFeedback[0] = escI2CISR.motorFeedback[1] = escI2CISR.motorFeedback[2] =
            escI2CISR.motorFeedback[3] = escI2CISR.motorFeedback[4] = escI2CISR.motorFeedback[5] =
            escI2CISR.motorFeedback[6] = 0;
        escI2CISR.numFeedbackCurrent = 0;

        I2C1CONSET = _I2C1CON_SEN_MASK;
        //I2CStart(I2C1);

        extern SemaphoreHandle_t semESCBus;
        if (xSemaphoreTake(semESCBus, 20) != pdTRUE || escI2CISR.state != STATE_STOPPED)
        {
            // uhoh
            escBusDisableISR();
            i2cChannelReset(0);
//            I2C1CONCLR = _I2C1CON_ON_MASK;
  //          portNOP();
    //        I2C1CONSET = _I2C1CON_ON_MASK;

            if (!escBootloader(i,0))
            {
                // error!
            }
            else
            {
                if (!escArm(i,1))
                {
                    // error!
                }
            }
            escBusEnableISR();
        }
        else
        {
            if (STATE.motors.mode == MOTOR_MODE_FLIGHT)
            {
                STATE.motors.setpoints[i].feedbackRPM = (escI2CISR.motorFeedback[0] << 8) | escI2CISR.motorFeedback[1];
                logStore(xTaskGetTickCount(), LOG_ID_MOTOR_1 + i, STATE.motors.setpoints[i]);
            }
            else if (STATE.motors.mode == MOTOR_MODE_DEBUG)
            {
                STATE.motors.tuning.setpoint = escI2CISR.motorSetpoint;
                STATE.motors.tuning.feedback[0] = (escI2CISR.motorFeedback[0] << 8) | escI2CISR.motorFeedback[1];
                STATE.motors.tuning.feedback[1] = (escI2CISR.motorFeedback[2] << 8) | escI2CISR.motorFeedback[3];
                STATE.motors.tuning.feedback[2] = (escI2CISR.motorFeedback[4] << 8) | escI2CISR.motorFeedback[5];
                logStore(xTaskGetTickCount(), LOG_ID_MOTOR_DEBUG, STATE.motors.tuning);
            }
            else if (STATE.motors.mode == MOTOR_MODE_BODE)
            {
                STATE.motors.bodeFeedback = escI2CISR.motorFeedback[0];
                STATE.motors.bode.error = (escI2CISR.motorFeedback[1] << 8) | escI2CISR.motorFeedback[2];
                STATE.motors.bode.pid = (escI2CISR.motorFeedback[3] << 8) | escI2CISR.motorFeedback[4];
                STATE.motors.bode.signal = (escI2CISR.motorFeedback[5] << 8) | escI2CISR.motorFeedback[6];
            }
        }
    }
    escBusDisableISR();
}


BYTE escBodeCalc()
{
    // error
    if (STATE.motors.bodeFeedback & 0x02)
    {
        //STATE.motors.mode = MOTOR_MODE_DEBUG;
        //escSetModeDebug(STATE.motors.debugSelected);
        STATE.motors.bodeFeedback = 0;
        return 2;
    }

    // received feedback
    if (STATE.motors.bodeFeedback & 0x01)
    {
        logStore(xTaskGetTickCount(), LOG_ID_MOTOR_BODE, STATE.motors.bode);
        bodeCalc(STATE.motors.bode.error,STATE.motors.bode.pid, STATE.motors.bode.signal);
    }
    STATE.motors.bodeFeedback = 0;
    
    if (STATE.bode.stepReceived == 0 && STATE.bode.durationCount >= STATE.bode.duration)
    {
        return 1;
    }

    return 0;
}


void TaskESC(void * params)
{
    struct CommandIn_t cmd;

    // preventing own i2c slave from responding.. bug? dunno.
    I2C1ADD = 0xFF;
    I2C1CONSET = _I2C1CON_A10M_MASK | _I2C1CON_STRICT_MASK;

    vTaskDelay(configTICK_RATE_HZ / 10);
    escScanBus();

    escSetModeOffDelay(configTICK_RATE_HZ / 3);

	for(;;)
	{
        extern QueueHandle_t queESCCommands;
        if (xQueueReceive(queESCCommands, &cmd, 0) == pdTRUE)
        {
            void (* func)() = commandGetFunction(cmd.command);

            if (func != NULL)
            {
                int i = 0;

                for (i=0; i < cmd.paramLength; i++)
                    escCmdBuffer[i] = rnGetParam(cmd.paramPosition + i);
                escCmdParams = cmd.paramLength;

                (*func)();
            }
        }

        if (STATE.motors.mode == MOTOR_MODE_BODE)
        {
            vTaskDelay(1);

            bodeSetValue(); // first injection
            escSetSpeed(); // set injection

            int beginTimeout = 100;

            for (;;)
            {
                if (i2cDetectESCPulse()) // wait for feedback pulse
                {
                    beginTimeout = -1;
                    bodeStep(); // increase step
                    bodeSetValue(); // get next value
                    escSetSpeed(); // set injection
                    BYTE result = escBodeCalc();
                    if (result == 1) // send feedback
                    {
                        if (!rnClaimTx(configTICK_RATE_HZ)) break;
                        BYTE * sendpacket = rnGetTxBuffer();
                        sendpacket[0] = CMD_MOTOR_BODE;
                        sendpacket[1] = STATE.motors.debugSelected;
                        sendpacket[2] = STATE.bode.frequency >> 8;
                        sendpacket[3] = STATE.bode.frequency & 0xFF;
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Re_cos_Error), sendpacket,4);
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Im_sin_Error), sendpacket,8);
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Re_cos_Out), sendpacket,12);
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Im_sin_Out), sendpacket,16);
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Re_cos_In), sendpacket,20);
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Im_sin_In), sendpacket,24);
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Re_cos_Signal), sendpacket,28);
                        INT_TO_BYTES_REV(*((int*)&STATE.bode.Im_sin_Signal), sendpacket,32);
                        rnTransmitAndConfirmBlock(sendpacket, 36);
                        rnReleaseTx();
                        break;
                    }
                    else if (result == 2)
                    {
                        // error
                        portNOP();
                        break;
                    }
                }
                else
                {
                    if (beginTimeout > 0)
                    {
                        beginTimeout--;
                        //vTaskDelay(1);
                        int i;
                        for (i=0;i<10000;i++) portNOP();
                        escSetSpeed(); // set injection
                    }
                    else
                    {
                        portNOP();
                        break;
                    }
                }
            }
            while (i2cDetectESCPulse());
            STATE.motors.mode = MOTOR_MODE_DEBUG;
            bodeStop();
            escSetSpeed();
            vTaskDelay(1);
        }
        else
        {
            escSetSpeed(); // set injection
            vTaskDelay(1);
        }
    }
}


BOOL i2cDetectESCPulse()
{
    I2C1CONCLR = _I2C1CON_ON_MASK;

    //I2CEnable(I2C1, FALSE);
    TRISASET = (1 << 15);
    //PORTSetPinsDigitalIn	(IOPORT_A, BIT_15);

    IFS0CLR = _IFS0_INT4IF_MASK;
    //INTClearFlag(INT_INT4);

    INTCONCLR = _INTCON_INT4EP_MASK; // Falling edge

    IFS0CLR = _IFS0_INT4IF_MASK;
    IEC0SET = _IEC0_INT4IE_MASK;

    //ConfigINT4(EXT_INT_ENABLE | FALLING_EDGE_INT | EXT_INT_PRI_5);
    BOOL result = xSemaphoreTake(semESCPulse, 5);

    IEC0CLR = _IEC0_INT4IE_MASK;
    //ConfigINT4(EXT_INT_DISABLE);
    //I2CEnable(I2C1, TRUE);

    I2C1CONSET = _I2C1CON_ON_MASK;

    return result;
}

void __attribute__((vector(_EXTERNAL_4_VECTOR), interrupt(ipl5))) ISRWRAPPER_INT4();
void ISR_INT4()
{
    static portBASE_TYPE higherPriorTask;

    IFS0CLR = _IFS0_INT4IF_MASK;
    //INTClearFlag(INT_INT4);

    xSemaphoreGiveFromISR(semESCPulse, &higherPriorTask);

    portEND_SWITCHING_ISR(higherPriorTask);
}


void __attribute__((vector(_I2C1_VECTOR), interrupt(ipl6))) ISRWRAPPER_I2C1();

void ISR_I2C1()
{
    static portBASE_TYPE higherPriorTask;



    //if (INTGetFlag(INT_SOURCE_I2C_ERROR(I2C1)))
    if (IFS0bits.I2C1BIF)
    {
        I2C1CONCLR = _I2C1CON_ON_MASK;
        //I2CEnable(I2C1,FALSE);
        IFS0CLR = _IFS0_I2C1MIF_MASK | _IFS0_I2C1BIF_MASK;
        //INTClearFlag(INT_SOURCE_I2C_ERROR(I2C1));
        //INTClearFlag(INT_SOURCE_I2C_MASTER(I2C1));
        i2cChannelReset(0);
        //I2CEnable(I2C1,TRUE);
        I2C1CONSET = _I2C1CON_ON_MASK;
    }

    if (IFS0bits.I2C1MIF)
//    if (INTGetFlag(INT_SOURCE_I2C_MASTER(I2C1)))
    {
        IFS0CLR = _IFS0_I2C1MIF_MASK;
        //INTClearFlag(INT_SOURCE_I2C_MASTER(I2C1));

        ESC_I2C_STATE nextState = escI2CISR.state + 1;
        extern SemaphoreHandle_t semESCBus;
        
        switch (escI2CISR.state)
        {
            case STATE_START:
                I2C1TRN = escI2CISR.motorAddress;
                //I2CSendByte(I2C1, escI2CISR.motorAddress);
                break;
            case STATE_ADDRESS:
//                if (!I2CByteWasAcknowledged(I2C1)) nextState = STATE_ERROR_STOP;
//                else I2CSendByte(I2C1, escI2CISR.motorSetpoint >> 8);
                if (I2C1STATbits.ACKSTAT) nextState = STATE_ERROR_STOP;
                else I2C1TRN = escI2CISR.motorSetpoint >> 8;
                break;
            case STATE_SETPOINT_MSB:
//                if (!I2CByteWasAcknowledged(I2C1)) nextState = STATE_ERROR_STOP;
//                else I2CSendByte(I2C1, escI2CISR.motorSetpoint & 0xFF);
                if (I2C1STATbits.ACKSTAT) nextState = STATE_ERROR_STOP;
                else I2C1TRN = escI2CISR.motorSetpoint & 0xFF;
                break;
            case STATE_SETPOINT_LSB:
                if (I2C1STATbits.ACKSTAT) nextState = STATE_ERROR_STOP;
                else I2C1CONSET = _I2C1CON_RSEN_MASK;
//                if (!I2CByteWasAcknowledged(I2C1)) nextState = STATE_ERROR_STOP;
//                else I2CRepeatStart(I2C1);
                break;
            case STATE_REPEAT_START:
//                I2CSendByte(I2C1, escI2CISR.motorAddress | 1);
                I2C1TRN = escI2CISR.motorAddress | 1;
                break;
            case STATE_ADDRESS_READ:
                if (I2C1STATbits.ACKSTAT) nextState = STATE_ERROR_STOP;
                else I2C1CONSET = _I2C1CON_RCEN_MASK;
//                if (!I2CByteWasAcknowledged(I2C1)) nextState = STATE_ERROR_STOP;
//                else I2CReceiverEnable(I2C1, TRUE);
                break;
            case STATE_READ:
//                escI2CISR.motorFeedback[escI2CISR.numFeedbackCurrent] = I2CGetByte(I2C1);
                escI2CISR.motorFeedback[escI2CISR.numFeedbackCurrent] = I2C1RCV;
                escI2CISR.numFeedbackCurrent++;
                if (escI2CISR.numFeedbackVars > escI2CISR.numFeedbackCurrent)
                {
                    I2C1CONCLR = _I2C1CON_ACKDT_MASK;
                    I2C1CONSET = _I2C1CON_ACKEN_MASK;
                }
//                   I2CAcknowledgeByte(I2C1, TRUE);
                else
                {
                    I2C1CONSET = _I2C1CON_ACKEN_MASK | _I2C1CON_ACKDT_MASK;
                    //I2CAcknowledgeByte(I2C1, FALSE);
                    nextState = STATE_STOP;
                }
                break;
            case STATE_READ_ACK:
                I2C1CONSET = _I2C1CON_RCEN_MASK;
                //I2CReceiverEnable(I2C1, TRUE);
                nextState = STATE_READ;
                break;
            case STATE_STOP:
                I2C1CONSET = _I2C1CON_PEN_MASK;
                //I2CStop(I2C1);
                break;
            case STATE_STOPPED:
                xSemaphoreGiveFromISR(semESCBus, &higherPriorTask);
                nextState = STATE_STOPPED;
                break;
            case STATE_ERROR:
                xSemaphoreGiveFromISR(semESCBus, &higherPriorTask);
                nextState = STATE_ERROR;
                break;
        }
        if (nextState == STATE_ERROR_STOP)
        {
            I2C1CONSET = _I2C1CON_PEN_MASK;
            //I2CStop(I2C1);
            nextState = STATE_ERROR;
        }
        escI2CISR.state = nextState;
    }

    portEND_SWITCHING_ISR(higherPriorTask);
}


void ESC_FIRMWARE_FLASH_CONFIG()
{

}

void ESC_BOOTLOADER()
{
    if (escCmdParams != 2) return;

    escBootloader(escCmdBuffer[0], escCmdBuffer[1]);

    if (!rnClaimTx(configTICK_RATE_HZ)) return;

    BYTE * packet = rnGetTxBuffer();
    packet[0] = CMD_ESC_BOOTLOADER;
    packet[1] = escCmdBuffer[0];
    packet[2] = STATE.motors.in_bootloader[escCmdBuffer[0]];
    rnTransmitAndConfirmBlock(packet, 3);
    rnReleaseTx();
}

void ESC_ADDRESSES()
{
    escScanBus();

    if (!rnClaimTx(configTICK_RATE_HZ)) return;

    BYTE * packet = rnGetTxBuffer();
    unsigned int pos = 0;

    packet[0] = CMD_ESC_ADDRESSES;
    int i;
    int c = 1;
    for (i=0;i<8;i++)
    {
        if (STATE.motors.address[i] == 0) continue;
        packet[c++] = STATE.motors.address[i];
        //SHORT_TO_BYTES(STATE.motors.version[i], packet, i*4+2);
        //packet[i*4+4] = STATE.motors.in_bootloader[i];
    }

    rnTransmitAndConfirmBlock(packet, c);

    rnReleaseTx();
}

#include <math.h>

void ESC_GET_DEBUG_FEEDBACK()
{
    if (escCmdParams != 3) return;

    if (escCmdBuffer[1] == 0)
    {
        //STATE.btLogging.varEnabled[LOG_ID_MOTOR_DEBUG] = 0;
        //escSetModeOff();
    }
    else
    {
        //STATE.btLogging.varEnabled[LOG_ID_MOTOR_DEBUG] = 1;
        //STATE.btLogging.disableSending = 0;
        //escSetModeDebug(escCmdBuffer[0]);
    }
    STATE.btLogging.varEnabled[LOG_ID_MOTOR_DEBUG] = 0;
    STATE.btLogging.varEnabled[LOG_ID_MOTOR_BODE] = 0;
    if (escCmdBuffer[2] == 1)
    {
        STATE.btLogging.varEnabled[LOG_ID_MOTOR_DEBUG] = 1;
    }
    else if (escCmdBuffer[2] == 2)
    {
        STATE.btLogging.varEnabled[LOG_ID_MOTOR_BODE] = 1;
    }


    if (!rnClaimTx(configTICK_RATE_HZ)) return;
    BYTE * packet = rnGetTxBuffer();
    packet[0] = CMD_ESC_GET_DEBUG_FEEDBACK;
    packet[1] = escCmdBuffer[0];
    packet[2] = (escCmdBuffer[2] == 0 ? 255 : (escCmdBuffer[2] == 1 ? LOG_ID_MOTOR_DEBUG : LOG_ID_MOTOR_BODE));
    rnTransmitAndConfirmBlock(packet, 3);
    rnReleaseTx();
}
